#include "roadengine_api.h"

#include "entities/WorldRoad.h"
#include "geom/RoadBuilder.h"

namespace roadengine {
/*
FWorldRefLineGraph::Ptr Build(const std::vector<FWorldRoad::Ptr>& roads,
                                    bool reverseX)
{
    FWorldRefLineGraph::Ptr reflines = std::make_shared<FWorldRefLineGraph>();

    bool s[3];
		s[0] = reverseX ? -1.0 : 1.0;
		s[1] = false ? -1.0 : 1.0;
		s[2] = false ? -1.0 : 1.0;

    FWorldEntityAxis axis;
	FWorldRefLineBuilder builder(reflines, axis,
			glm::dmat4x4(
				s[0],	0.0,	0.0,	0.0,
				0.0,  s[1],  	0.0,	0.0,
				0.0,	0.0,   s[2],	0.0,
				0.0,	0.0,	0.0,	1.0
			));
		builder.Reset(roads);
		builder.Process();

        return reflines;
}

void BuildRoadMap(const std::vector<FWorldRoad::Ptr>& roads,
                  std::map<std::int64_t, FWorldRoad::Ptr>& roadMap)
{
    for (auto road: roads) {
      roadMap[road->ID.Get()] = road;
    }
}

static rg::ReferenceLine SampleSpline(const FWorldRefLineSpline::Ptr spline, const glm::dvec3& offset, double threshold)
{
	rg::ReferenceLine ret;
	//int cvNum = spline->IntervalCount() + 1;
	double splineLen = spline->Length();
	int sampleCnt = int(splineLen / threshold);
	if (sampleCnt == 0)
		sampleCnt = 1;

	//double refPtDis = 0;
	for (int ci = 0; ci <= sampleCnt; ++ci) {

		double curDis = (double)ci / (double)sampleCnt * splineLen;
		glm::dvec3 p_interp = spline->EvalAtDistance(curDis) - offset;
		glm::dvec3 t_interp = spline->EvalTangentAtDistance(curDis);
		// This is to workaround a defect in UE spline evaluation, probably caused by having to spline points nearly at the same location.
		if (glm::length(t_interp) < (threshold * 0.0001f))
			continue;

		glm::vec2 roadRef_P(p_interp.x, p_interp.y);
		glm::vec2 roadRef_T(t_interp.x, t_interp.y);
		roadRef_T = glm::normalize(roadRef_T);
		// TODO: need to handle road elevation.
		ret.samples.push_back({ glm::vec3(p_interp), roadRef_T, curDis, 1.0f });
	}
	return ret;
}

void test(const std::string& odFilePath, const std::string& roadId)
{
    std::unique_ptr<FODWorldData> odWorld = ImportODWorld(odFilePath);
   
    std::shared_ptr<FWorldRefLineGraph> reflines = Build(odWorld, false);
    std::map<std::string, FWorldRoad::Ptr> roadMap;
    BuildRoadMap(odWorld->CachedRoads, roadMap);

    if (roadMap[roadId]) {
        auto road = roadMap[roadId];
        FWorldRefLineSpline::Ptr spline = (*reflines)[road];

        std::shared_ptr<rg::RoadGeometry> roadGeom = std::make_shared<rg::RoadGeometry>();

        double localThreshold = 0.2;
        glm::dvec3 offset(0.0, 0.0, 0.0);
        rg::ReferenceLine roadRefLine = SampleSpline(spline, offset, localThreshold);

		roadGeom->Initialize(roadRefLine, localThreshold);
		roadGeom->GenerateLaneLines();

        const std::vector<SectionLaneDivLines>& divLines = roadGeom->mCachedLaneSectionDivLines;
        //typedef std::map<float, LaneDivRefLine> SectionLaneDivLines;
        for (size_t i = 0; i < divLines.size(); i++) {
            // iterator each lane
            const SectionLaneDivLines& divLine = divLines[i];

            //for (auto& lineEntity: divLine ) {

            //}
        }
    }

}

bool GetRoadData2(const std::string& filename, 
                 std::map<std::int64_t, rg::RoadGeometry::Ptr>& roadGeometries)
{
    std::unique_ptr<FODWorldData> odWorld = ImportODWorld(filename);

    if (!odWorld) {
		printf("Failed to load OpenDrive file.\n");
		return false;
	}
   
    FWorldRefLineGraph::Ptr reflines = Build(odWorld->CachedRoads, false);
    std::map<std::int64_t, FWorldRoad::Ptr> roadMap;
    BuildRoadMap(odWorld->CachedRoads, roadMap);

    for (auto item: roadMap) {
        std::int64_t roadId = item.first;
        auto& road = item.second;
        FWorldRefLineSpline::Ptr spline = (*reflines)[road];

        std::shared_ptr<rg::RoadGeometry> roadGeom = std::make_shared<rg::RoadGeometry>();

        double localThreshold = 0.2;
        glm::dvec3 offset(0.0, 0.0, 0.0);
        rg::ReferenceLine roadRefLine = SampleSpline(spline, offset, localThreshold);

		roadGeom->Initialize(roadRefLine, localThreshold);
		roadGeom->GenerateLaneLines();

        const std::vector<SectionLaneDivLines>& divLines = roadGeom->mCachedLaneSectionDivLines;
        //typedef std::map<float, LaneDivRefLine> SectionLaneDivLines;
        for (size_t i = 0; i < divLines.size(); i++) {
            // iterator each lane
            const SectionLaneDivLines& divLine = divLines[i];

            for (auto& lineEntity: divLine ) {
               double distance = lineEntity.first;
               const rg::LaneDivRefLine& refLine = lineEntity.second;

               printf("sample size: %d\n", refLine.samples.size());
               for (size_t i = 0; i < refLine.samples.size(); ++i) {
		            const rg::RefPoint& curSamp = refLine.samples[i];
                    printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);
               }
            }
        }
    }

    return true;
}
*/

bool GetRoadData(const std::string& content, bool reverseX,
                 std::map<std::int64_t, rg::RoadGeometry::Ptr>& roadGeometries)
{
    std::unique_ptr<FODWorldData> loadedWorld = ImportODWorld(content);
    //auto loadedWorld = ImportODWorld(filename);
	if (!loadedWorld) {
		printf("Failed to load OpenDrive file.\n");
		return false;
	}

	// Convert the OpenDrive file into geometry data
	bool axisCvtFlag[3] = {reverseX, false, false};
	std::unique_ptr<rg::RoadBuilder> builtData(new rg::RoadBuilder(loadedWorld->EntityAxis, loadedWorld->EntityManager));
	builtData->BindOrCreate(loadedWorld->CachedRoads, axisCvtFlag);

	// Now the converted geometry data can be used for rendering(after invoking function Finalize)
	for (auto& road : builtData->_roadGeometries) {

        //if (road.first->ID.Get() == 2398) {
        //    printf("road 2398");
        //}
        
        //road.second.roadGeom->mOptimizeGoemetry = false;
		road.second.roadGeom->Finalize();

        roadGeometries[road.first->ID.Get()] = road.second.roadGeom;
/*
        if (road.first->ID.Get() != 2398) {
            continue;
        }
typedef std::map<float, rg::LaneDivRefLine> SectionLaneDivLines;

        glm::dvec3 origin = road.second.roadGeom->mLocalOrigin;
        printf("origin: %f, %f\n", origin[0], origin[1]);
        const std::vector<SectionLaneDivLines>& divLines = road.second.roadGeom->mCachedLaneSectionDivLines;
        //typedef std::map<float, LaneDivRefLine> SectionLaneDivLines;
        for (size_t i = 0; i < divLines.size(); i++) {
            // iterator each lane
            const SectionLaneDivLines& divLine = divLines[i];

            for (auto& lineEntity: divLine ) {
               double distance = lineEntity.first;
               const rg::LaneDivRefLine& refLine = lineEntity.second;

               printf("distance: %f, sample size: %d\n", distance, refLine.samples.size());
               for (size_t i = 0; i < refLine.samples.size(); ++i) {
		            const rg::RefPoint& curSamp = refLine.samples[i];
                    printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);
                    
               }

               if (refLine.isDriving) {
                      printf("mid sample number: %d\n", refLine.laneMidSamples.size());
                      for (size_t i = 0; i < refLine.laneMidSamples.size(); ++i) {
                        const rg::RefPoint& curSamp = refLine.laneMidSamples[i];
                        printf("sample point: (%f, %f)\n", curSamp.P[0], curSamp.P[1]);                        
                        }    
               }
            }
        }
        */
    }

    return true;
}

}